#include <stdio.h>
#include <unistd.h>

#include "display.h"
#include "simulator.h"
#include "iRobotSlam.h"
#include "MarkerLocator.h"

int main(void)
{
/*#ifdef GUI
	Mat q = (Mat_<double>(2,1) << .05, M_PI/180);
	Mat s = (Mat_<double>(2,1) << 1, 5*M_PI/180);
	Simulator sim(q,s);

	Mat u = (Mat_<double>(2,1) << .4, M_PI/24);

	while(1) {
		sim.step(u);
		if(cvWaitKey(20) == 27)
			break;
	}
#endif
	FileStorage fs("config.yml", FileStorage::READ);
	Mat cam, dist, rpos;

	fs["cameraMatrix"] >> cam;
	fs["distMatrix"] >> dist;
	fs["sensorPos"] >> rpos;

	fs.release();

	MarkerLocator ml(cam, dist, rpos, 100.);

	VideoCapture cap(0);
	while(1) {
		Mat frame;
		cap >> frame;
		Mat bwFrame;
		cvtColor(frame, bwFrame, CV_BGR2GRAY);

		imshow("frame", frame);
		ml.UpdateMarkers(bwFrame);
		if(waitKey(20) ==27)
			break;
	}*/

	startSlam(1);
	return 0;
}
